#include <iostream>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <stdlib.h>
#include <sstream>

//TODO for some reason, comm only works when using ttyACM1, not ttyACM0
#define USB_SERIAL_PORT "/dev/ttyACM1" // (Linux Ubuntu Mac Book)

//char sensor_buf[1023];
int port_fd;

std::string currentLine= " ";

int gyroX;
int gyroY;
int gyroZ;

int accelX;
int accelY;
int accelZ;

int magX;
int magY;
int magZ;

int init_serial_input (char * port) {
  int fd = 0;
  struct termios options;

  std::cout << "Made Termios" << std::endl;

  fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);

  std::cout << "Opened fd USB port" << std::endl;

  if (fd == -1)
    return fd;
    fcntl(fd, F_SETFL, 0);    // clear all flags on descriptor, enable direct I/O
    tcgetattr(fd, &options);   // read serial port options
    // enable receiver, set 8 bit data, ignore control lines
    options.c_cflag |= (CLOCAL | CREAD | CS8);
    // disable parity generation and 2 stop bits
    options.c_cflag &= ~(PARENB | CSTOPB);
    // set the new port options
    tcsetattr(fd, TCSANOW, &options);
    return fd;
}

void read_serial_xyz (int fd) {
	char sensor_buf[5*sizeof(int)+14*sizeof(char)];

		read(fd, &sensor_buf, 5*sizeof(int)+14*sizeof(char));
		//sleep(.005);
//
//			if(sensor_buf[0]=='C'){
//				std::cout <<sensor_buf<<std::endl;//[i++];
//			}
//			else{
				std::cout <<sensor_buf; //<<std::endl;
//			}
		//		std::cout << "reading port" << std::endl;
		//	}

	currentLine= sensor_buf;
}

void parseXYZ(std::string line){



	// Find the location of the space character
	int space = line.find(" ");

	if(space<0){
		std::cout<<"space is not there"<<std::endl;
	}
	std::string split_after_column_1 = line.substr(space+1);

	int space2= split_after_column_1.find(" ");
	if(space2<0){
		std::cout<<"space2 is not there"<<std::endl;
	}
	std::string split_after_column_2 = split_after_column_1.substr(space2+1);

	int space3= split_after_column_2.find(" ");
	if(space3<0){
		std::cout<<"space3 is not there"<<std::endl;
	}


	// This is a little tricky so you have to think about it logically
	std::stringstream(line.substr(0, space))>> gyroX;	//x is the number before the fist space
	if(space2>0){
		std::stringstream(split_after_column_1.substr(0, space2))>> gyroY; //y is the number before the 2nd space
	}
	if(space3>0){
		std::stringstream(split_after_column_2.substr(space2+1))>> gyroZ; //z is the number after the 2nd space
	}

}


//void sound() {
//std::cout << "going to read" << std::endl;
//  std::cout<<read_serial_xyz(port_fd)<<std::endl;
//  std::cout << "done" << std::endl;
//
//}


int main( int argc, char** argv  )
{
	FILE *file;
	//file = fopen("/dev/ttyUSB1", "w");
	//std::string currentLine= " ";
	port_fd = init_serial_input(USB_SERIAL_PORT);
	// serial is checked

	if (port_fd == -1)  {
		std::cout << "Couldn't open USB port" << std::endl;
	}

	else{
		while(1){
			int go= 0;
			//std::cout<<"enter number to get value"<<std::endl;
			//			std::cin >> go;
			//			file = fopen(USB_SERIAL_PORT, "w");
			//			std::cout<<"opened port"<<std::endl;
			//
			//			switch(go){
			//			case 1:
			//				fprintf(file,"%c",'x');
			////				sleep(.05);
			//				for(int i=0;i<100;i++){
			//				fprintf(file,"%c",' ');
			//				}
			//				break;
			//			case 2:
			//				fprintf(file,"%c",'y');
			////				sleep(.05);
			//				for(int i=0;i<100;i++){
			//					fprintf(file,"%c",' ');
			//					}
			//				break;
			//			case 3:
			//				fprintf(file,"%c",'z');
			//				fprintf(file,"%c",' ');
			//				break;
			//			}
			//
			//			std::cout<<"printed it"<<std::endl;
			//			fclose(file);
			//			std::cout<<"closed it"<<std::endl;


			//	for(int i=0; i<3;i++){
			read_serial_xyz(port_fd);

			//std::cout<<currentLine<<std::endl;
			//		parseXYZ(currentLine);
			//		std::cout<<"X: "<<gyroX<<" Y: "<<gyroY<<" Z: "<<gyroZ<<std::endl;
			//}
			//std::cout<<"\n";
			//sleep(.02);
		}

	}
}
